#include "ant.hpp"
#include "error.hpp"
#include "random.hpp"
#include "trace.hpp"
#include <numeric>
#include <climits>
#include <cstring>
#include <cstdio>

#define TIME_SCALE                              10000
#define RIGID_EDGE_PENALTY                      10000000
#define PHEROMONE_MODIFIER                      1.0

ant_c::~ant_c ( void )
{
        if ( NULL != route )
        {
                delete route;
                route = NULL;
        }

        if ( NULL != colour_classes )
        {
                colour_classes_v::iterator i;
                for ( i = colour_classes->begin (); i != colour_classes->end (); i ++ )
                {
                        colour_class_l * colour_class = ( colour_class_l * ) * i;
                        delete colour_class;
                }
                delete colour_classes;
                colour_classes = NULL;
        }
}

ant_c::ant_c ( void ) :
        colour_bound            ( 0 ),
        graph                   ( NULL ),
        palette                 ( NULL ),
        current_colour          ( -1 ),
        max_colour              ( -1 ),
        route                   ( NULL ),
        route_is_legal          ( false ),
        tour_time               ( LONG_MAX ),
        stage_id                ( -1 ),
        state                   ( ANT_STATE_INVALID ),
        colour_classes          ( NULL )
{
        route = new ant_route_v ();
        // TODO: consider instantiating colour class
}

ant_c::ant_c ( const ant_c & right ) :
        colour_bound ( right.colour_bound )
{
        * this = right;
}

ant_c::ant_c ( graph_c * graph, palette_c *  palette, long colour_bound ) :
        colour_bound            ( colour_bound ),
        graph                   ( graph ),
        palette                 ( palette ),
        current_colour          ( 0 ),
        current_path            ( NULL ),
        current_stage_time      ( 0 ),
        max_colour              ( 0 ),
        route                   ( NULL ),
        route_is_legal          ( false ),
        tour_time               ( 0 ),
        stage_id                ( 0 ),
        state                   ( ANT_STATE_IDLE ),
        colour_classes          ( NULL )
{
        throw_if ( NULL == graph, ERROR_INVALID_ARGUMENTS );
        throw_if ( NULL == palette, ERROR_INVALID_ARGUMENTS );

        route = new ant_route_v ();
        route->reserve ( graph->get_nodes_count () );

        colour_classes = new colour_classes_v ( colour_bound, NULL );
        for ( unsigned i = 0; i < colour_classes->size (); i ++ )
        {
                colour_classes->at ( i ) = new colour_class_l ();
        }
}

void ant_c::restart_tour ( void )
{
        state                   = ANT_STATE_IN_TOUR;
        stage_id                = 0;
        tour_time               = 0;
        current_path            = NULL;
        current_stage_time      = 0;
        route_is_legal          = true;

        max_colour              = colour_bound - 1;
        current_colour          = random_c::get ().next ( colour_bound );

        route->clear ();
        route->push_back ( current_colour );

        colour_classes_v::iterator i;
        for ( i = colour_classes->begin (); i != colour_classes->end (); i ++ )
        {
                colour_class_l * colour_class = ( colour_class_l * ) * i;
                colour_class->clear ();
        }
}

void ant_c::find_next_colour ( void )
{
        palette_c::paths_v *    paths                   = palette->get_paths ( stage_id, current_colour );
        long                    paths_count             = get_colours_count ();
        palette_c::pheromone_t  pheromone_sum           = std::accumulate ( paths->begin (), paths->end (), 0 );
        probabilities_v         path_probabilities;

        throw_if ( ( unsigned long ) paths_count > paths->size (), ERROR_STATE_UNEXPECTED );
        path_probabilities.reserve ( paths_count );
        for ( palette_c::paths_v::iterator i = paths->begin (); i != paths->end (); i ++ )
        {
                palette_c::pheromone_t  pheromone       = ( palette_c::pheromone_t ) * i;
                double                  probability     = ( 1.0 + pheromone ) / ( paths_count + pheromone_sum );      
                path_probabilities.push_back ( probability );
        }

        long next_colour = random_c::get ().wheel_of_fortune ( path_probabilities );

        route->push_back ( next_colour );
        current_path = & paths->at ( next_colour );
        current_colour = next_colour;
}

void ant_c::measure_stage_time ( void )
{
        long current_node = stage_id + 2 /* + 2 */;
        current_stage_time = 0;

        // Iterate through colour class list to which belonges current node
        colour_class_l::iterator i;
        colour_class_l * colour_class = colour_classes->at ( current_colour );
        for ( i = colour_class->begin (); i != colour_class->end (); i ++ )
        {
                long that_node = ( long ) * i;
                const edge_c * edge = graph->get_edge ( that_node, current_node );

                if ( NULL != edge )
                {
                        if ( edge->is_rigid () )
                        {
                                route_is_legal = false;       
                                current_stage_time += RIGID_EDGE_PENALTY;
                                break;
                        }
                        else
                        {
                                current_stage_time += ( long ) ( edge->get_probability () * TIME_SCALE + 0.5 );
                        }
                }
        }

        /*for ( long i = 1; i < current_node; i ++ )
        {
                long that_node_colour = route->at ( i - 1 );
                if ( current_colour == that_node_colour )
                {
                        const edge_c * edge = graph->get_edge ( i, current_node );

                        if ( NULL != edge )
                        {
                                if ( edge->is_rigid () )
                                {
                                        route_is_legal = false;       
                                        current_stage_time += RIGID_EDGE_PENALTY;
                                        break;
                                }
                                else
                                {
                                        current_stage_time += ( long ) ( edge->get_probability () * TIME_SCALE + 0.5 );
                                        //::trace ( TRACE_LEVEL_LOW, "%d %d %d %f %d\n",  current_colour, i, current_node, edge->get_probability (), current_stage_time);
                                }
                        }
                }
        }*/
        tour_time += current_stage_time;

        // Add current node to colour class list
        colour_class->push_back ( current_node );
}

void ant_c::create_pheromone_path ( void )
{
        if ( tour_time < RIGID_EDGE_PENALTY )
                * current_path += PHEROMONE_MODIFIER / ( 1 + current_stage_time );
}

void ant_c::after_stage_state_update ( void )
{               
        if ( tour_time >= RIGID_EDGE_PENALTY )
                restart_tour ();
        else
                if ( ++ stage_id >= palette->get_levels_count () - 1 )
                        state = ANT_STATE_COMPLETED;
}

void ant_c::run_next_stage ( void )
{
        find_next_colour ();
        measure_stage_time ();
        create_pheromone_path ();
        after_stage_state_update ();
}

const ant_route_v * ant_c::get_route ( void ) const
{
        return route;
}

long ant_c::get_tour_time ( void ) const
{
        return tour_time;
}

bool ant_c::tour_completed ( void ) const
{
        return ANT_STATE_COMPLETED == state;
}

long ant_c::get_colours_count ( void ) const
{
        return max_colour + 1;
}

bool ant_c::colouring_is_valid ( void ) const
{
        return route_is_legal;
}

void ant_c::trace ( trace_level_e trace_level ) const
{
        const long max_message_length = 80;
        long chars_left = max_message_length - 4;
        char message [ max_message_length ];
        sprintf ( message, "ant_c: tour time=%ld, route=", tour_time );
        chars_left -= strlen ( message );
        for ( ant_route_v::const_iterator i = route->begin (); i != route->end (); i ++ )
        {
                long colour = ( long ) * i;
                char appendix [ max_message_length ];
                sprintf ( appendix, "%ld ", colour );
                chars_left -= strlen ( appendix );
                if ( chars_left >= 0 )
                {
                        strcat ( message, appendix );
                }
                else
                {
                        ::trace ( trace_level, "ant_c: route size is too big for ant trace\n" );
                        break;
                }
                        
        }
        strcat ( message, "\n" );
        ::trace ( trace_level, message );
}

ant_c & ant_c::operator= ( const ant_c & right )
{
        colour_bound    = right.colour_bound;
        graph           = right.graph;
        palette         = right.palette;
        current_colour  = right.current_colour;
        max_colour      = right.max_colour;
        * route         = * right.route;
        route_is_legal  = right.route_is_legal;
        tour_time       = right.tour_time;
        stage_id        = right.stage_id;
        state           = right.state;
        // TODO: implement colour_classes assignment
        return * this;
}

